using System;
using System.Collections;
using Microsoft.SPOT;

using Gadgeteer.Networking;
using GT = Gadgeteer;
using GTM = Gadgeteer.Modules;
using Gadgeteer;
using Gadgeteer.Interfaces;

namespace Dairy_Town
{
    class Accelerometer
    {
        private ArrayList xPoints = new ArrayList();
        private ArrayList yPoints = new ArrayList();
        private ArrayList zPoints = new ArrayList();

        private AnalogInput accelInputX;
        private AnalogInput accelInputY;
        private AnalogInput accelInputZ;

        private GTM.GHIElectronics.Extender extender;

        private bool debugging = false;

        private double baseX;
        private double baseY;
        private double baseZ;

        private double _X;
        private double _Y;
        private double _Z;

        /// <summary>
        /// Returns the acceleration measured on the X-Axis in m/s^2
        /// </summary>
        public double X
        {
            get
            {
                return _X;
            }
        }

        /// <summary>
        /// Returns the acceleration measured on the Y-Axis in m/s^2
        /// </summary>
        public double Y
        {
            get
            {
                return _Y;
            }
        }

        /// <summary>
        /// Returns the acceleration measured on the Z-Axis in m/s^2
        /// </summary>
        public double Z
        {
            get
            {
                return _Z;
            }
        }

        public Accelerometer(GTM.GHIElectronics.Extender instance)
        {
            this.extender = instance;

            Socket socket = Gadgeteer.Socket.GetSocket(10, true, null, null);
            accelInputX = extender.SetupAnalogInput(Socket.Pin.Five);
            accelInputY = extender.SetupAnalogInput(Socket.Pin.Four);
            accelInputZ = extender.SetupAnalogInput(Socket.Pin.Three);

            this.calibrate();
        }

        private void calibrate()
        {
            int tries = 0;
            GT.Timer poll_Calibration = new GT.Timer(250);
            poll_Calibration.Tick += (GT.Timer timer) =>
            {
                tries++;

                if (tries >= 3)
                {
                    timer.Stop();
                    this.collect();
                }

                baseX = accelInputX.ReadProportion();
                baseY = accelInputY.ReadProportion();
                baseZ = accelInputZ.ReadProportion() - 0.1;

                if (debugging)
                {
                    Debug.Print("Base X:" + baseX);
                    Debug.Print("Base Y:" + baseY);
                    Debug.Print("Base Z:" + baseZ);
                }
            };
            poll_Calibration.Start();
        }

        private void collect()
        {
            GT.Timer poll_Accelerometer = new GT.Timer(100);
            poll_Accelerometer.Tick += poll_Accelerometer_Tick;
            poll_Accelerometer.Start();
        }

        public void debugEnabled(bool value)
        {
            debugging = value;
        }

        int tickCount = 0;

        private void poll_Accelerometer_Tick(GT.Timer timer)
        {
            double inputX = accelInputX.ReadProportion();
            double inputY = accelInputY.ReadProportion();
            double inputZ = accelInputZ.ReadProportion();

            double gForce = 9.8; // Acceleration of Gravity

            double accelX = ((baseX - inputX) * 10) * gForce;
            double accelY = ((baseY - inputY) * 10) * gForce;
            double accelZ = ((baseZ - inputZ) * 10) * gForce;

            if (debugging)
            {
                Debug.Print("X: " + accelX + " m/s^2" + "(" + inputX + ")");
                Debug.Print("Y: " + accelY + " m/s^2" + "(" + inputY + ")");
                Debug.Print("Z: " + accelZ + " m/s^2" + "(" + inputZ + ")");
            }

            xPoints.Add(accelX);
            yPoints.Add(accelY);
            zPoints.Add(accelZ);

            tickCount++;

            if (tickCount >= 10)
            {
                double lowX = 0;
                double lowY = 0;
                double lowZ = 0;
                double highX = 0;
                double highY = 0;
                double highZ = 0;
                
                
                for (int i = 0; i < xPoints.Count; i++)
                {
                /*
                 * Loop through the last 10 values and determines the 
                 * most extreme action for the second, either acceleration 
                 * or deacceleration. Did not use sort function in order to allow 
                 * the possibility of capturing specific index at which extreme occurred.
                 * 
                 */

                    double comparatorX = (double)xPoints[i];
                    double comparatorY = (double)yPoints[i];
                    double comparatorZ = (double)zPoints[i];

                    if (i == 0)
                    {
                        lowX = comparatorX;
                        highX = comparatorX;

                        lowY = comparatorY;
                        highY = comparatorY;

                        lowZ = comparatorZ;
                        highZ = comparatorZ;

                        continue;
                    }

                    if (comparatorX > highX)
                    {
                        highX = comparatorX;
                    }

                    if (comparatorX < lowX)
                    {
                        lowX = comparatorX;
                    }

                    if (comparatorY > highY)
                    {
                        highY = comparatorY;
                    }

                    if (comparatorY < lowY)
                    {
                        lowY = comparatorY;
                    }

                    if (comparatorZ > highZ)
                    {
                        highZ = comparatorZ;
                    }

                    if (comparatorZ < lowZ)
                    {
                        lowZ = comparatorZ;
                    }

                    /*
                     *Grabs the most extreme, Acceleration or Deacceleration 
                     */
                    _X = ( System.Math.Abs(highX) > System.Math.Abs(lowX) ) ? highX : lowX;
                    _Y = ( System.Math.Abs(highY) > System.Math.Abs(lowY) ) ? highY : lowY;
                    _Z = ( System.Math.Abs(highZ) > System.Math.Abs(lowZ) ) ? highZ : lowZ;
                }
                
                /* 
                 * Clear the last 10 values (1-second of data)
                 */
                tickCount = 0;
                xPoints.Clear();
                yPoints.Clear();
                zPoints.Clear();
            }
        }
    }
}
